import time
import gevent.socket
import robomaster.robot
import socket


def calculate_angle(direction1, direction2):
    angles = {
        "right": 0,
        "left": 180,
        "up": 90,
        "down": 270
    }

    angle1 = angles.get(direction1, None)
    angle2 = angles.get(direction2, None)

    if angle1 is None or angle2 is None:
        return None

    angle_difference = angle1 - angle2

    if angle_difference == 270:
        angle_difference = -90
    elif angle_difference == -270:
        angle_difference = 90

    return angle_difference


def process_duplicates(lst):
    # 找出所有重复点及其索引
    seen = {}
    duplicates = {}
    for idx, point in enumerate(lst):
        if point not in seen:
            seen[point] = idx
        else:
            if point in duplicates:
                duplicates[point].append(idx)
            else:
                duplicates[point] = [seen[point], idx]

    # 计算所有重复点的索引差值，并找出差值最大的重复点
    max_diff = 0
    max_diff_point = None
    for point, indexes in duplicates.items():
        diff = indexes[1] - indexes[0]
        if diff > max_diff:
            max_diff = diff
            max_diff_point = point

    # 删除除了差值最大的重复点以外的所有重复点，并将差值最大的重复点的第二次出现删除
    result = []
    for idx, point in enumerate(lst):
        if point in duplicates and point != max_diff_point:
            continue
        if point == max_diff_point and idx == duplicates[max_diff_point][1]:
            continue
        result.append(point)
    return result


class MazeSolver:
    def __init__(self, start, end, drone):
        """
        迷宫类初始化实例
        :param start:起点
        :param end: 终点
        :param drone: 无人机对象
        """
        self.end_direct = None
        self.drone: robomaster.robot.Drone = drone
        self.sensor: robomaster.robot.sensor.TelloDistanceSensor = self.drone.sensor
        self.led: robomaster.robot.led.TelloLed = self.drone.led
        self.flight: robomaster.flight.Flight = self.drone.flight
        self.start = start
        self.end = end
        self.height = 6
        self.width = 5
        self.paths = []
        self.directions_ = {(0, 1): "up", (1, 0): "right", (-1, 0): "left", (0, -1): "down"}
        self.directions_up = [("right", (1, 0)), ("up", (0, 1)), ("left", (-1, 0)), ("down", (0, -1))]
        self.directions_down = [("left", (-1, 0)), ("down", (0, -1)), ("right", (1, 0)), ("up", (0, 1))]
        self.directions_left = [("up", (0, 1)), ("left", (-1, 0)), ("down", (0, -1)), ("right", (1, 0))]
        self.directions_right = [("down", (0, -1)), ("right", (1, 0)), ("up", (0, 1)), ("left", (-1, 0))]
        self.dictionary = {"up": self.directions_up, "right": self.directions_right, "left": self.directions_left, "down": self.directions_down}
        self.correction = {(1, 1): (60, 90), (2, 1): (0, 90), (3, 1): (-60, 90),
                           (1, 2): (60, 30), (2, 2): (0, 30), (3, 2): (-60, 30),
                           (1, 3): (60, -30), (2, 3): (0, -30), (3, 3): (-60, -30),
                           (1, 4): (60, -90), (2, 4): (0, -90), (3, 4): (-60, -90)}
        self.found = False

    def rotate_and_move(self, angle, nx, ny):
        """
        无人机前进逻辑
        :param ny: 无人机将要进入的x坐标
        :param nx: 无人机将要进入的y坐标
        :param angle:旋转角度
        """
        self.rotate_retry(angle=angle)
        while True:
            tof = self.sensor.get_ext_tof()
            # print("目前无人机与前方障碍物的距离：", tof)
            if tof is None:
                continue
            else:
                if tof > 600:
                    self.flight.forward(distance=60).wait_for_completed(timeout=3)
                    self.correct_pos(nx, ny)
                    return True
                elif tof <= 120:
                    self.flight.backward(20).wait_for_completed(timeout=5)
                    return False
                else:
                    return False

    def get_keys(self, val):
        for key, value in self.correction.items():
            if val == value:
                return key
        return "There is no key"

    def correct_pos(self, nx, ny):
        """
        无人机矫正位置
        :param my:
        :param mx:
        :param nx: 坐标点x
        :param ny: 坐标点y
        """
        x, y = self.correction.get((nx, ny))
        mid = self.drone.get_status('mid')
        if mid == 12:
            mid_s = 'm12'
            self.go_retry(x, y, mid_s)
        elif mid == 1:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm1'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="1")
            self.go_retry(0, 0, mid_s)
        elif mid == 2:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm2'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="2")
            self.go_retry(0, 0, mid_s)
        elif mid == 3:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm3'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="3")
            self.go_retry(0, 0, mid_s)
        elif mid == 4:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm4'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="4")
            self.go_retry(0, 0, mid_s)
        elif mid == 5:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm5'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="5")
            self.go_retry(0, 0, mid_s)
        elif mid == 6:
            mx, my = self.correction.get((nx, ny))
            rx, ry = self.get_keys((mx, my))
            mid_s = 'm6'
            print(f"识别到挑战卡m{mid},当前坐标点为({rx - 1},{ry - 1})")
            self.led.set_mled_char(display_char="6")
            self.go_retry(0, 0, mid_s)

    def go_retry(self, x, y, mid):
        action: robomaster.flight.FlightAction = self.flight.go(x=x, y=y, z=80, mid=mid, speed=40)
        action.wait_for_completed(timeout=5)
        print("go is completed?:" + str(action.is_completed))
        while not action.is_completed:
            action: robomaster.flight.FlightAction = self.flight.go(x=x, y=y, z=80, mid='m12', speed=40)
            action.wait_for_completed(timeout=5)

    def rotate_retry(self, angle):
        """
        无人机旋转函数
        :param angle: 旋转角度
        """
        action: robomaster.flight.FlightAction = self.flight.rotate(angle=angle)
        action.wait_for_completed(timeout=5)
        print("cw is completed?:" + str(action.is_completed))
        while not action.is_completed:
            action: robomaster.flight.FlightAction = self.flight.rotate(angle=angle)
            action.wait_for_completed(timeout=5)

    def dfs(self, x, y, directions):
        """
        迷宫探索-深度优先搜索
        :param x: 无人机当前x坐标
        :param y: 无人机当前y坐标
        :param directions: 探索方向序列
        """
        if self.found:
            return
        for direction, (dx, dy) in directions:
            # 结束条件：到达终点
            if self.found:
                return

            # 向前走之后的坐标点
            nx, ny = x + dx, y + dy

            print("当前方向：", direction, "将要进入的点：", (nx, ny))

            # 转向
            if direction == directions[0][0]:
                access = self.rotate_and_move(90, nx, ny)
            else:
                access = self.rotate_and_move(-90, nx, ny)

            # 第一层逻辑：是否超出边界
            if 0 <= nx < self.height and 0 <= ny < self.width:
                # 第二层逻辑：是否有障碍
                if access:
                    # 将向前走之后现如今的坐标点入队列
                    if direction == directions[-1][0]:
                        self.paths.pop()

                    self.paths.append((nx, ny))

                    # print(self.paths)
                    if (nx, ny) == self.end:
                        print("探索结束")
                        self.end_direct = direction
                        self.found = True

                    # 递归
                    self.dfs(nx, ny, self.dictionary.get(direction))
                    # 子层递归如果一二层逻辑进不去的话，上一层递归就会执行到这一步

                    if self.found:
                        return

    def backtrack(self, paths, direction):
        """
        无人机回溯最短路径
        :param paths: 最短路径列表
        :param direction: 无人机当前朝向
        """
        while len(paths) > 1:
            second_to_last = paths[-2]
            first_to_last = paths[-1]
            print(second_to_last, first_to_last)
            (dx, dy) = (int(second_to_last[0]) - int(first_to_last[0]), int(second_to_last[1]) - int(first_to_last[1]))
            go_direct = self.directions_.get((dx, dy))
            angle = calculate_angle(direction, go_direct)
            self.rotate_retry(angle=angle)
            self.flight.forward(distance=60).wait_for_completed()
            self.correct_pos(second_to_last[0], second_to_last[1])
            time.sleep(2)
            direction = go_direct
            paths.pop()

        self.led.set_led(r=0, g=0, b=255)
        self.flight.land().wait_for_completed()

    def solve(self):
        self.led.set_led(r=0, g=255, b=0)
        self.paths.append(self.start)
        self.dfs(self.start[0], self.start[1], self.directions_down)
        self.paths = process_duplicates(self.paths)
        print("探索到的最短路径：", self.paths)
        self.led.set_led(r=255, g=0, b=0)
        time.sleep(0.1)
        self.led.set_led(r=0, g=0, b=0)
        time.sleep(0.5)
        self.led.set_led(r=255, g=0, b=0)
        time.sleep(0.1)
        self.led.set_led(r=0, g=0, b=0)
        time.sleep(0.5)
        self.led.set_led(r=255, g=0, b=0)
        time.sleep(0.1)
        self.led.set_led(r=0, g=0, b=0)
        time.sleep(0.5)
        self.led.set_led(r=0, g=255, b=0)
        self.backtrack(self.paths, self.end_direct)


if __name__ == "__main__":
    robomaster.config.LOCAL_IP_STR = "192.168.10.2"
    tl_drone = robomaster.robot.Drone()
    tl_drone.initialize()

    tl_flight: robomaster.flight.Flight = tl_drone.flight
    tl_flight.takeoff().wait_for_completed()

    solver = MazeSolver((2, 4), (2, 1), tl_drone)
    solver.solve()
